import numpy as np
import cv2
import socket
import threading
import time

from getAngle import getAngle
from getAngle import getL

resultA = []
resultB = []
flagA = False
flagB = False

ip_filename = "/home/pi/workspace/stereo-cv/config/ip.txt"
ipServer = ""
with open(ip_filename, "r") as f:  # 打开文件
    data = f.readline()  # 读取文件
    datas = data.split(" ")
    ipServer = datas[1].split("\n")[0]
    f.close()

ADDRESS = (ipServer, 8000)
server_socket_A = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket_A.bind(ADDRESS)
server_socket_A.listen(2)

ADDRESS = (ipServer, 6000)
server_socket_B = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket_B.bind(ADDRESS)
server_socket_B.listen(2)

def thread1(HOST, PORT):
    global flagA, resultA

    print("thread1 start")

    while True:
        print("client 1 waitting......")
        client_socket_A, client_address = server_socket_A.accept()
        print("client 1 connect succeed!")
        try:
            while True:
                # 接收标志数据
                data = client_socket_A.recv(1024)
                if data:
                    # 通知客户端“已收到标志数据，可以发送图像数据”
                    client_socket_A.send(b"ok")
                    # 处理标志数据
                    flag = data.decode().split(",")
                    # 图像字节流数据的总长度
                    if flag[0] == "result":
                        # print("-------")
                        total = int(flag[1])
                        cnt = 0
                        bytes = b""
                        data = b""
                        while(cnt < total):
                            bytes = client_socket_A.recv(256000)
                            data += bytes
                            cnt += len(bytes)
                        data_split = data.decode().split(",")
                        cnt = 0
                        resultA0 = []
                        while cnt < len(data_split) - 3:
                            resultA0.append([float(data_split[cnt]), float(data_split[cnt + 1]), float(data_split[cnt + 2])])
                            cnt += 3
                        # print(len(resultA))
                        resultA = resultA0
                        file_handle = open('receive_points_A.txt', 'w+')
                        for result in resultA:
                            file_handle.write("%f %f %f\n" \
                                % (result[0], result[1], result[2]))
                        file_handle.close()
                        resultA = np.array(resultA)
                        client_socket_A.send(b"ok")
                        flagA = True
                        break
                    else:
                        total = int(flag[0])
                        # 接收到的数据计数
                        cnt = 0
                        # 存放接收到的数据
                        img_bytes = b""

                        while cnt < total:
                            # 当接收到的数据少于数据总长度时，则循环接收图像数据，直到接收完毕
                            data = client_socket_A.recv(256000)
                            img_bytes += data
                            cnt += len(data)
                            # print("receive:" + str(cnt) + "/" + flag[0])
                        # 通知客户端“已经接收完毕，可以开始下一帧图像的传输”
                        client_socket_A.send(b"ok")

                        # 解析接收到的字节流数据，并显示图像
                        img = np.asarray(bytearray(img_bytes), dtype="uint8")
                        img = cv2.imdecode(img, cv2.IMREAD_COLOR)
                        cv2.imshow("img", img)
                        cv2.waitKey(1)
                else:
                    print("Disconnected!")
                    break
        finally:
            client_socket_A.close()


def thread2(HOST, PORT):  
    global flagB, resultB
    print("thread2 start")  
    while True:
        print("client 2 waitting......")
        client_socket_B, client_address = server_socket_B.accept()
        print("client 2 connect succeed!")
        try:
            while True:
                # 接收标志数据
                data = client_socket_B.recv(1024)
                if data:
                    # 通知客户端“已收到标志数据，可以发送图像数据”
                    client_socket_B.send(b"ok")
                    # 处理标志数据
                    flag = data.decode().split(",")
                    # 图像字节流数据的总长度
                    if flag[0] == "result":
                        # print("-------")
                        total = int(flag[1])
                        cnt = 0
                        bytes = b""
                        data = b""
                        while(cnt < total):
                            bytes = client_socket_B.recv(256000)
                            data += bytes
                            cnt += len(bytes)
                        data_split = data.decode().split(",")
                        cnt = 0
                        resultB0 = []
                        while cnt < len(data_split) - 3:
                            resultB0.append([float(data_split[cnt]), float(data_split[cnt + 1]), float(data_split[cnt + 2])])
                            cnt += 3
                        # print(len(resultB))
                        resultB = resultB0
                        file_handle = open('receive_points_B.txt', 'w+')
                        for result in resultB:
                            file_handle.write("%f %f %f\n" \
                                % (result[0], result[1], result[2]))
                        file_handle.close()
                        resultB = np.array(resultB)
                        client_socket_B.send(b"ok")
                        flagB = True
                        break
                    
                    else:
                        total = int(flag[0])
                        # 接收到的数据计数
                        cnt = 0
                        # 存放接收到的数据
                        img_bytes = b""

                        while cnt < total:
                            # 当接收到的数据少于数据总长度时，则循环接收图像数据，直到接收完毕
                            data = client_socket_B.recv(256000)
                            img_bytes += data
                            cnt += len(data)
                            # print("receive:" + str(cnt) + "/" + flag[0])
                        # 通知客户端“已经接收完毕，可以开始下一帧图像的传输”
                        client_socket_B.send(b"ok")

                        # 解析接收到的字节流数据，并显示图像
                        img = np.asarray(bytearray(img_bytes), dtype="uint8")
                        img = cv2.imdecode(img, cv2.IMREAD_COLOR)
                        cv2.imshow("img_b", img)
                        cv2.waitKey(1)
                else:
                    print("Disconnected!")
                    break
        finally:
            client_socket_B.close()


threads = []
t1 = threading.Thread(target=thread1, args=('169.254.220.88', 8000))
threads.append(t1)
t2 = threading.Thread(target=thread2, args=('169.254.220.88', 6000))
threads.append(t2)


for t in threads:
    t.setDaemon(True)
    t.start()

while True:
    if flagA and flagB:
        # 计算theta
        # print("lenA: " + str(len(resultA)))
        # print("lenB: " + str(len(resultB)))
        angle = 90 - getAngle(resultA, resultB)
        LA = getL(resultA)
        LB = getL(resultB)
        L = 0
        if(angle < 45):
            L = LB
        else:
            L = LA
        print("---------")
        print("angle: " + str(angle))
        print("L    : " + str(L))
        print("---------")
        flagA = False
        flagB = False

        HOST = '127.0.0.1'
        PORT = 8000
        ADDRESS = (HOST, PORT)
        tcpClient = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # 创建一个套接字
        tcpClient.connect(ADDRESS) # 连接机器人
        bytes = (str(L)).encode() + " ".encode() + (str(angle)).encode()
        tcpClient.send(bytes) # 发送指令
        


